#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <sys/time.h>
#include <libusb.h>

#include "tlink_usb.h"
#include "usbprn.h"

#if defined(__clang__)
# pragma clang diagnostic push
#endif

#if defined(__clang__)
# pragma clang diagnostic ignored "-Wcast-align"
#endif

#define dev_handle tlink_dev


int WriteToDevice(libusb_device_handle * hd, uint16_t deviceAddress, uint16_t length, uint8_t * writeBuffer) 
{

	int r;
	//unsigned int i;

	uint8_t type = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_INTERFACE;
	char buff[4096];

	uint8_t	bmRequestType = type;
	uint8_t	bRequest = 0x02;
	uint16_t wValue = 0xf0;
	uint16_t wIndex = 0;
	uint16_t wLength = length + 8;
	unsigned int timeout = 100;
	char *	data = buff;

	buff[0] = 0x02;
	buff[1] = deviceAddress >> 8;
	buff[2] = deviceAddress;
	buff[3] = buff[4] = buff[5] = buff[6] = buff[7] = writeBuffer[0]; // !! Question: ???
	memcpy (buff + 8, writeBuffer, length);


	LOG_DEBUG("write len %d\n", length);
	data[length] = '\0';

	// returns: on success, the number of bytes actually transferred
	// Ref: http://libusb.sourceforge.net/api-1.0/group__syncio.html#gadb11f7a761bd12fc77a07f4568d56f38
	//r = libusb_control_transfer(hd, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout);
	r = tlink_libusb_control_transfer(hd, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout);

	if (r < 0) {
		LOG_USER("write to endpoint0 error %d\n", r);
		return r;
	}

	if ((unsigned int) r < sizeof(data)) {
		LOG_DEBUG("short write (%d)\n", r);
		return -1;
	}

	return r>=0 ? 0 : -1;
}

int ReadFromDevice(libusb_device_handle *hd, uint16_t deviceAddress, uint16_t length, uint8_t *writeBuffer, int *nr)
{
	int r;
	//unsigned int i;

	uint8_t type = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_INTERFACE;
	uint8_t	bmRequestType = type;
	uint8_t	bRequest = 0x02;
	uint16_t wValue = deviceAddress;
	uint16_t wIndex = 0;
	char *	data = (char *) writeBuffer;
	uint16_t wLength = length + 8;
	unsigned int timeout = 100;

	r = tlink_libusb_control_transfer(hd, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout);
	if (r < 0) {
		LOG_USER("F0 error %d\n", r);
		return r;
	}
	if ((unsigned int) r < sizeof(data)) {
#ifdef TDEBUG
		LOG_USER("short read (%d)\n", r);
#endif
		return -1;
	}

#ifdef TDEBUG
	LOG_DEBUG("Readed data:");
	for (int i = 0; i < (int)sizeof(data); i++) 
		printf("%02x ", data[i]);
	LOG_USER("\n");
#endif

	if (nr) {
		*nr = r;
	}
	return r;
}


//================================================================================================
//
//	SignalHandler
//
//	This routine will get called when we interrupt the program (usually with a Ctrl-C from the
//	command line).
//
//================================================================================================
void SignalHandler(int sigraised) 
{
	LOG_USER("User Interrupted %d", sigraised);

	tlink_usb_close();

	exit((int)0);
}

int GetHandle()
{
	sig_t					oldHandler;

	// Set up a signal handler so we can clean up when we're interrupted from the command line
	// Otherwise we stay in our run loop forever.
	//
	oldHandler = signal(SIGINT, SignalHandler);
	if (oldHandler == SIG_ERR) {
		LOG_USER("Could not establish new signal handler.");
	}

	if (tlink_usb_open() == NULL) {
		LOG_DEBUG("Cant find Telink EVK");
		return ERROR_FAIL;
	}


#ifdef TDEBUG
	uint32_t dr = 0x1234;
	int retval = tlink_usb_control_write(dev_handle, 0, 2, (uint8_t *)&dr);
	WriteMem(dev_handle, 0, (uint8_t *)&dr, 2, 2);
	dr = 0;
	retval = tlink_usb_control_read (dev_handle, 0, 4, (uint8_t *)&dr, 0);
	LOG_DEBUG("USB Read: 0x%8.8x\n", dr);
#endif
	
	return ERROR_OK;
}


int IsEVKDevice (libusb_device_handle * devh)
{
	uint32_t id;

	LOG_DEBUG("Handle 0x%08x", (uint32_t)devh);
	
	ReadUSBMem(devh, 0x7c, (unsigned char*) &id, 4);

	LOG_DEBUG("isEVKdevice reading id   = 0x%08x", id );

	if ((id & 0xffff0000) == 0x08180000)
		return 4;
	else
		return 0;
}

#ifndef WIN32
unsigned int GetTickCount (void)
{
	struct timeval tv;
	gettimeofday(&tv,NULL);
	return (unsigned int)(tv.tv_sec*1000 + (tv.tv_usec / 1000));
}
#endif
int EVKCommand (libusb_device_handle *hdev, int addr, int len, int cmd, /*uint8_t * lpB,*/ int * ptick)
{
	uint32_t cmdbuf[16];
	cmdbuf[0] = addr;
	cmdbuf[1] = (len & 0xffff) | (cmd<<16);

	LOG_DEBUG("EVKCommand addr %#04x cmd 0x%04x len %d", addr, cmd, len);
	LOG_DEBUG("cmdbuf[0] = 0x%08x", cmdbuf[0]);
	LOG_DEBUG("cmdbuf[1] = 0x%08x", cmdbuf[1]);

	int ret = WriteUSBMem(hdev, 0x9ff8, (uint8_t *) cmdbuf, 8);

	int rlen = len;

	if (!ret) 
		return 0;

	unsigned int ts = GetTickCount ();
	unsigned int tl = 1000;

	do {
		ret = ReadUSBMemCheck(hdev, 0x9ff0, (uint8_t *) cmdbuf, 16, 1024);
		LOG_DEBUG("cmdbuf[0] = 0x%08x", cmdbuf[0]);

		if (cmdbuf[0]&0xf0000000) {
			return 0;
		}
		else if (cmdbuf[0] == (uint32_t)rlen) {
			if (ptick) 
				*ptick = cmdbuf[1];
			LOG_DEBUG("found it 2");
			return len;
		}
		else if (cmdbuf[0]) {
		}
	} while (GetTickCount() - ts < tl);

	return ERROR_OK;
}



int WriteUSBMem(libusb_device_handle *hdev, int addr, uint8_t * lpB, int len /*, int maxlen*/)
{
	LOG_DEBUG("WriteUSBMem 0x%8.8X len %d", (uint32_t)addr, len);
	return tlink_usb_control_write (hdev, addr, len, (unsigned char*)lpB);
}

int ReadUSBMem(libusb_device_handle *hdev, int addr, uint8_t * lpB, int len /*, int maxlen*/)
{
	LOG_DEBUG("ReadUSBMem 0x%8.8X len %d", (uint32_t)addr, len);
	int nr;
	tlink_usb_control_read(hdev, addr, len, (unsigned char*)lpB, &nr);
	return nr;
}

int ReadUSBMemCheck(libusb_device_handle *hdev, int addr, uint8_t * lpB, int len, int maxlen)
{

	int	nBytes;

	int nR = 8 + (len > maxlen ? maxlen :  len<1 ? 1 : len);

	int retry = (nR + 8) / 4;
	int bRet;

	while (retry > 0 || nR>0)
	{
		retry --;

		bRet = tlink_usb_control_read(hdev, addr, len, (uint8_t *)lpB, &nBytes);


		if (!bRet) {
			LOG_DEBUG("control_read returned %d", bRet);
			return 0;
		} else if (nBytes == nR - 8) {
			LOG_DEBUG("ReadUSBMemCheck 0x%08x len %d nBytes %d nR %d", (uint32_t)addr, len,nBytes,nR);
			//tlink_debug_buffer(lpB, nBytes);
			return len;
		}	        
		nR -= nBytes;
		lpB += nBytes;
		addr += nBytes;
        nBytes = 0;
	}

	LOG_DEBUG("ReadUSBMemCheck retried %d\n", retry);

	return ERROR_OK;
}

int WriteUSBMemCheck(libusb_device_handle *hdev, int addr, uint8_t * lpB, int len, int maxlen)
{
	BYTE rbuff[4096];
	int nc = 10;
	int l = 0;
	int retw, retr;

	while (l++<nc) {
		retw = WriteUSBMem (hdev, addr, lpB, len);
		if (len <= 8)
			return retw;

		int ok = 1;
		retr = ReadUSBMemCheck  (hdev, addr, rbuff, len, maxlen);

		for (int k=0; k<len; k++) {
			if (lpB[k] != rbuff[k]) {
				ok = 0;
				LOG_DEBUG("USB write adrress %x", addr + k);
				break;
			}
		}
		return retw;

		if (ok)
			return retw;
	}
	return ERROR_OK;
}

#define		UCMD_CORE		0x0100
#define		UCMD_FRD		0x8000
#define		UCMD_FSWIRE		0x4000

int WriteMem(libusb_device_handle *hdev, int addr, uint8_t * lpB, int len, int type)
{

	//BYTE ah = addr>>16;
	int ret = 0;

	LOG_DEBUG("WriteMem 0x%x len %d type %d", addr, len, type);

	int al = addr & 0xffff;
	int step = 1024;

	for (int i=0; i<len; i+=step) {

		int n = len - i > step ? step : len - i;
		int fadr = al + i;

		if (type & 0x200) 
				fadr = al;
		int rw;

		if ( (type & 15) != 4)
			rw = WriteUSBMemCheck (hdev, fadr, lpB+i, n, /*type & 0x200,*/ step);
		else {
			rw = WriteUSBMem (hdev, 0xa000, lpB+i, n);

			if (rw == n)
				rw = EVKCommand (hdev, fadr, n, UCMD_CORE|UCMD_FSWIRE, /*lpB+i,*/ NULL);
			else
				return ret;
		}

		if (rw)
			ret += rw;
		else
			break;
	}
	return ret;
}

int ReadMem(libusb_device_handle *hdev, int addr, uint8_t * lpB, int len, int type)
{

	//BYTE ah = addr>>16;
	int ret = 0;

	int al = addr & 0xffff;
	//int step = 32;
	int step = 3072;

	LOG_DEBUG("ReadMem handle %8.8X", (uint32_t)hdev);

	for (int i=0; i<len; i+=step) {

		int n = len - i > step ? step : len - i;
		int fadr = al + i;

		if (type & 0x200) 
			fadr = al;
		
		if ( (type & 15) != 4) {
			ret += ReadUSBMemCheck (hdev, fadr, lpB+i, n, /*type & 0x200,*/ step);
		}
		else {
			if (EVKCommand (hdev, fadr, n, UCMD_CORE|UCMD_FSWIRE|UCMD_FRD, /*lpB+i,*/ NULL) == n) {
				ret += ReadUSBMemCheck (hdev, 0xa000, lpB+i, n, /*type & 0x200,*/ step);
			}
			else
				return ret;
		}
	}
	return ret;
}

int WriteUSBReg(libusb_device_handle *hdev, int addr, int dat)
{

	BYTE buff[16];
	*((int *) buff) = dat;

	LOG_DEBUG("WriteUSBReg 0x%8.8X %d", addr, dat);
	int bRet = WriteUSBMem(hdev, addr, buff, 1);

	return bRet;
}

int ReadUSBReg(libusb_device_handle *devh, int addr, int * dat)
{


	BYTE buff[16];
	LOG_DEBUG("Handle 0x%8.8X 0x%2X", (uint32_t)devh, addr);
	int bRet = ReadUSBMem(devh, addr, buff, 1);
	* dat = buff[0];

	LOG_DEBUG("ReadUSBReg 0x%8.8X 0x%2X", addr, *dat);

	return bRet;

}
#if defined(__clang__)
# pragma clang diagnostic pop
#endif
